/*
 * CameraStateValidityChecker.h
 *
 *  Created on: Mar 22, 2013
 *      Author: ohnozzy
 */

#ifndef CAMERASTATEVALIDITYCHECKER_H_
#define CAMERASTATEVALIDITYCHECKER_H_
#include <vector>
#include <ompl/base/StateValidityChecker.h>
#include <fcl/collision.h>
#include <fcl/shape/geometric_shapes.h>

#include "CameraSpace.h"
#include "VehiclePath.h"
namespace flyingcamera
{
    namespace base
    {


       class CameraStateValidityChecker : public ompl::base::StateValidityChecker
       {
       protected:
    	   VehiclePath* path;
    	   fcl::Cone robot;
    	   std::vector<fcl::Transform3f> obstacle_tf;
    	   std::vector<fcl::CollisionGeometry*> obstacle;
       public:
          CameraStateValidityChecker(const ompl::base::SpaceInformationPtr &si, VehiclePath* p, double camera_distance, double base_r) :
                    ompl::base::StateValidityChecker(si), path(p), robot(base_r, camera_distance)
           {
           }
          virtual bool isValid(const ompl::base::State *state) const;
          void addObstacle(fcl::CollisionGeometry* const o, const fcl::Transform3f&  tf);
         };
    }
}



#endif /* CAMERASTATEVALIDITYCHECKER_H_ */
